import lejos.nxt.*;
import lejos.robotics.navigation.DifferentialPilot;

public class Toad implements SensorPortListener {
  DifferentialPilot pilot = new DifferentialPilot(5.6f, 11.f, Motor.A, Motor.B);
  int wall = 0;
  int day;
  
  public Toad() {
    SensorPort.S1.setType(SensorConstants.TYPE_SWITCH);
    SensorPort.S4.setType(SensorConstants.TYPE_SWITCH);
    SensorPort.S2.setType(SensorConstants.TYPE_LIGHT_INACTIVE);
    SensorPort.S1.addSensorPortListener(this);
    SensorPort.S2.addSensorPortListener(this);
    SensorPort.S4.addSensorPortListener(this);
    day = SensorPort.S2.readValue();
  }
  
  public void go() {
    pilot.forward();
  }

  public void stateChanged( SensorPort port , int oldValue , int newValue ) {
    System.out.println(port.getId() + ": " + oldValue + "->" + newValue);
    if (port == SensorPort.S2) {
      if (newValue > 1.1*day)
	pilot.stop();
      else
	pilot.steer(wall*-50);
      return;
    }
    if (oldValue < newValue)
      return; // nothing to do here
    if (wall == 0)
      wall = port == SensorPort.S1 ? 1 : -1;
    pilot.travel(-2);
    pilot.rotate(wall*90);
    pilot.steer(wall*-50);
  }

  public static void main(String[] args) {
    Toad me = new Toad();
    me.go();
    Button.waitForAnyPress();
  }
}